/*
	TOPCAT Payload Software, for use in the UKube-1 CubeSat mission.
	
	http://www.bath.ac.uk/elec-eng/invert/topcat.html
	
	Written by:
	Mark Jessop, University of Adelaide
	Talini Pinto Jayawardena, University of Bath
	
	Uses extracts from source code provided by Clyde Space for I2C communications.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    For a full copy of the GNU General Public License, 
    see <http://www.gnu.org/licenses/>.
*/

#include <Mega_SPI.h>
#include <spieeprom.h>


#define PIN_3V3_SW  5
#define PIN_LED		18 // On TOPCAT board
uint8_t LED_STATE = 0;
#define PIN_WATCHDOG	9

#define SERIAL_DEBUG 1

#define EEPROM_ENABLED 1


// Packet Spec Setting.
// OLD_ACK = 0 for new spec (Revision F), OLD_ACK = 1 for older specs (Revision E?)
#define OLD_ACK	0

//////////////////////////////////////////////
// GPS Settings and Global Variables / Defines
//////////////////////////////////////////////
#define GPS_PORT Serial2

#define GPS_BUFFER_SIZE 650 // Set this to the Max packet size.

// GPS Flag Byte.
volatile unsigned char GPS_FLAGS = 0x00;

// GPS Flags
#define GPS_SATLOCK 0  //Flag to indicate sat lock confirmation sent to platform
#define GPS_MODESET 1  //Flag to indicate an op mode is set (non-zero)
#define GPS_DATA_AVAILABLE 2
#define GPS_FOUND_HEADER 3
#define GPS_COMPLETE_PACKET 4
#define GPS_INITIALISED	5
#define GPS_NOLOCK	6
#define GPS_MAX_MEM 7

#define NOLOCK_RESTART 605000 // If no lock in 10 minutes, restart.

// GPS Global Variables
volatile unsigned char GPS_SATS = 0;
volatile unsigned int msg_id = 0;

unsigned long gps_startup_timer = 0;


// GPS Buffer Struct
struct gps_buffer
{
  unsigned char buffer[GPS_BUFFER_SIZE];
  unsigned int ptr;
  unsigned char message_flags;
  unsigned int message_length;
  unsigned int message_id;
  unsigned int seq_num;
};

gps_buffer * gps_packet; // The packet we will be reading from.
unsigned int gps_sequence_number = 0;

SPIEEPROM storage(1);
byte eeprom_buffer[256]; // EEPROM Buffer.
byte eeprom_write_buffer[256];
unsigned int eeprom_write_ptr = 0;
byte eeprom_next_read_page[256]; // Buffer the next page to be read. Maybe we won't miss timing deadlines this way...
uint8_t EEPROM_FLAGS = 0x00;
#define EEPROM_PAGE_READY	0
#define EEPROM_BROKEN	1

#define MAX_PAGES 511 // 512 x 256-byte pages in the 25LC1024. Use the last one for storing head and tail on powerdown.
#define PRIORITY_THRESHOLD 	32	// When we only have this many pages remaining, inform platform.

//////////////////////////////////////////////

//////////////////////////////////////////////
// I2C Settings and Global Variables / Defines
//////////////////////////////////////////////
#define PAY_ADDR  0x71 // University of Bath address - 0x62

#define I2C_BUFSIZE   265

// I2C Flag Byte
volatile unsigned char I2C_FLAGS = 0x00;

#define I2C_NEW_PACKET 	0
#define I2C_TX_BUSY		1
#define I2C_TX_PACKET_READY 2

// I2C Rx and Tx buffers
uint8_t i2c_rxd_buffer[I2C_BUFSIZE];
uint8_t i2c_txd_buffer[I2C_BUFSIZE];
unsigned int i2c_tx_length;


//
// Global error flag
//
uint8_t ERROR_FLAGS = 0x00;


//
// Global Status Information
//

uint8_t PAYLOAD_INITIALIZED = 0;

uint8_t PAYLOAD_OP_MODE = 0x00;
unsigned int PAYLOAD_OP_FLAGS = 0x0000;
unsigned int PAYLOAD_PRIORITY_DATA_WAITING = 0;
unsigned int PAYLOAD_DATA_WAITING = 0;
uint8_t PAYLOAD_REQUEST_FLAGS = 0x00;
#define REQUEST_MODE_CHANGE 1
#define REQUEST_RESTART	6
#define REQUEST_SHUTDOWN 7
#define REQUEST_UPDATE 0

uint8_t PAYLOAD_PARAMETER_1 = 0;
uint8_t PAYLOAD_PARAMETER_2 = 0;
uint8_t PAYLOAD_PARAMETER_3 = 0;
uint8_t PAYLOAD_PARAMETER_4 = 0;
uint8_t PAYLOAD_PARAMETER_5 = 0;
uint8_t PAYLOAD_PARAMETER_6 = 0;
uint8_t PAYLOAD_PARAMETER_7 = 0;
uint8_t PAYLOAD_PARAMETER_8 = 0;

unsigned int ORBIT_MAX_MEM = 0;

uint16_t		PAYLOAD_PRIORITY_DATA_LIMIT = 0;
uint16_t		PAYLOAD_PRIORITY_DATA_REMAINING = 0;
unsigned long	PAYLOAD_MASS_MEMORY_LIMIT = 0;
unsigned long	PAYLOAD_MASS_MEMORY_REMAINING = 0;
unsigned long	INIT_PAYLOAD_MASS_MEMORY_REMAINING = 0;

void setup() {
	pinMode(PIN_WATCHDOG,OUTPUT);
	pinMode(PIN_LED,OUTPUT); // Debug LED
	kick_watchdog();
	

    #if defined(SERIAL_DEBUG)
		Serial.begin(115200); // Debug Serial
		Serial.println("Booting up.");
    #endif
    
    #if defined(EEPROM_ENABLED)
    	#if defined(SERIAL_DEBUG)
    		Serial.println("Starting EEPROM.");
    	#endif
    	storage.setup();
    	EEPROM_test();
    	EEPROM_restore_state();
    	//EEPROM_store_state();
    #endif
    #if defined(SERIAL_DEBUG)
    	Serial.println("Starting I2C and dropping to main loop.");
    #endif
    
    I2C_begin();
    
    bitClear(PORTD, 0);
    bitClear(PORTD, 1); 

}

void loop() {
	kick_watchdog();

	// I2C Packet Handling
  	if(bitRead(I2C_FLAGS, I2C_NEW_PACKET)){
  		I2C_process_rx_packet();
  	}

	if(PAYLOAD_INITIALIZED == 1){
	  // put your main code here, to run repeatedly:
	  	
		// Send the GPS init commands after 5 seconds.
		if( ((millis() - gps_startup_timer) > 5000) && bitRead(GPS_FLAGS,GPS_INITIALISED)==0 ){
			oemv_init();
			bitSet(GPS_FLAGS,GPS_INITIALISED);
			#if defined(SERIAL_DEBUG)
				Serial.println("GPS Initialised");
			#endif
		}
		
		if( (millis() - gps_startup_timer) > NOLOCK_RESTART && GPS_SATS<4 && (bitRead(GPS_FLAGS,GPS_NOLOCK)==0)){
			bitSet(PAYLOAD_REQUEST_FLAGS,REQUEST_RESTART);
			bitSet(GPS_FLAGS, GPS_NOLOCK);
			#if defined(SERIAL_DEBUG)
				Serial.println("No Lock");
			#endif
		}else{
			if((bitRead(GPS_FLAGS,GPS_MODESET)==0) && (GPS_SATS>=4) && bitRead(PAYLOAD_REQUEST_FLAGS,REQUEST_MODE_CHANGE)==0){
		        bitSet(PAYLOAD_REQUEST_FLAGS,REQUEST_MODE_CHANGE); // Request a mode change.
		  		#if defined(SERIAL_DEBUG)
		   			Serial.println("Got GPS lock. Requesting mode change.");
		  		#endif
			}
		}
		
         
		if ( (bitRead(GPS_FLAGS,GPS_MODESET)==1) && (bitRead(GPS_FLAGS,GPS_MAX_MEM)==0) && (PAYLOAD_MASS_MEMORY_REMAINING == (INIT_PAYLOAD_MASS_MEMORY_REMAINING - (ORBIT_MAX_MEM * 256) ) ) ){
			#if defined(SERIAL_DEBUG)
				Serial.println("No Memory. Requesting Shutdown.");
			#endif
			oemv_shutdown();
			bitSet(PAYLOAD_REQUEST_FLAGS,REQUEST_SHUTDOWN);
			bitSet(GPS_FLAGS,GPS_MAX_MEM);
		}
		else{
	
		  if(bitRead(GPS_FLAGS,GPS_DATA_AVAILABLE) && bitRead(GPS_FLAGS,GPS_MODESET)){
			bitClear(GPS_FLAGS, GPS_DATA_AVAILABLE);
			
			// Copy data in gps buffer to EEPROM.
			kick_watchdog();
			
			new_GPS_copy_to_eeprom();
			
			// Debug Stuff.
			
			#if defined(SERIAL_DEBUG)
				kick_watchdog();
				Serial.print("RX, ID: ");
				Serial.print(gps_packet->message_id,DEC);
				Serial.print(" Len: ");
				Serial.print(gps_packet->message_length,DEC);
				Serial.print(" Seq: ");
				Serial.print(gps_packet->seq_num,DEC);
				Serial.print(" LB: ");
				Serial.println(gps_packet->buffer[gps_packet->message_length-1],HEX);
			  
				if((GPS_SATS!= 0) && (gps_packet->message_id == 42)){
					Serial.print("Sats: ");
					Serial.println(GPS_SATS,DEC);
				}
				kick_watchdog();
			#endif
		  }
		}
		
		if(PAYLOAD_PARAMETER_7 == 1){
			// Erase EEPROM
			#if defined(EEPROM_ENABLED)
				#if defined(SERIAL_DEBUG)
    				Serial.print("Got EEPROM Erase Command from Platform... ");
    			#endif
				kick_watchdog();
				EEPROM_erase();
				#if defined(SERIAL_DEBUG)
    				Serial.println("Erased.");
    			#endif
			#endif	
			PAYLOAD_PARAMETER_7 = 0;
		}
		#if defined(EEPROM_ENABLED)
			if( (bitRead(EEPROM_FLAGS,EEPROM_PAGE_READY)==0) && (EEPROM_used() > 0) ){
				kick_watchdog();
				EEPROM_buffer_page();
				#if defined(SERIAL_DEBUG)
					kick_watchdog();
					Serial.println("Buffered EEPROM Page into RAM.");
					kick_watchdog();
				#endif
			}
		#endif
		
		
	}
}

void new_GPS_copy_to_eeprom(){

	gps_buffer * temp_packet = gps_packet;
	
	#if defined(SERIAL_DEBUG)
		Serial.print("Got GPS Data, len: ");
	#endif
	
	unsigned int msg_len = gps_packet->message_length;
	
	#if defined(SERIAL_DEBUG)
		Serial.println(msg_len, DEC);
	#endif
	
	// Check the GPS packet's size. If too long, either write part of it or discard
	if(msg_len > ((unsigned long)(MAX_PAGES - EEPROM_used()) * 256)){
		#if defined(SERIAL_DEBUG)
			Serial.print("Not enough space for complete EEPROM Write: ");
		#endif
		if(eeprom_write_ptr<255){
			if(eeprom_write_ptr == 0){
				eeprom_write_buffer[0] = (uint8_t)gps_sequence_number>>8;
				eeprom_write_buffer[1] = (uint8_t)gps_sequence_number;
				gps_sequence_number = (gps_sequence_number + 1)& 0x0FFF;
				eeprom_write_ptr = 2;
			}
			// Writeout as much of the packet as possible into the packet, then return.
			unsigned int i;
			while(eeprom_write_ptr < 256){
				eeprom_write_buffer[eeprom_write_ptr++] = gps_packet->buffer[i++]; 
			}
			#if defined(SERIAL_DEBUG)
				Serial.println("Filled packet.");
			#endif
		}else{
			#if defined(SERIAL_DEBUG)
				Serial.println("Discarded data.");
			#endif
		}
		return;
	}
	
	// Else, write it into eeprom.
	unsigned int packet_count = 0;
	//for(packet_count = 0; packet_count<msg_len; packet_count++){
	while(packet_count<msg_len){
	
		if(eeprom_write_ptr == 256){
			EEPROM_write_page();
			EEPROM_update_data_waiting();
			#if defined(SERIAL_DEBUG)
				Serial.print("Wrote Packet to EEPROM, Seq No: ");
				Serial.println(gps_sequence_number - 1, DEC);
			#endif
			eeprom_write_ptr = 0;
			kick_watchdog();
		}
		if(eeprom_write_ptr == 0){
			eeprom_write_buffer[0] = (uint8_t)gps_sequence_number>>8;
			eeprom_write_buffer[1] = (uint8_t)gps_sequence_number;
			gps_sequence_number = (gps_sequence_number + 1)& 0x0FFF;
			eeprom_write_ptr = 2;
		}
		eeprom_write_buffer[eeprom_write_ptr++] = gps_packet->buffer[packet_count++];
		if(eeprom_write_ptr==128){kick_watchdog();}
		#if defined(SERIAL_DEBUG)
			//Serial.println(eeprom_write_ptr, DEC);
		#endif
	}
}

void kick_watchdog(){
	digitalWrite(PIN_WATCHDOG, HIGH);
	delayMicroseconds(20); // Is this really required?
	digitalWrite(PIN_WATCHDOG, LOW);
	
	if(LED_STATE = 0){
		digitalWrite(PIN_LED,HIGH);
		LED_STATE = 1;
	}else{
		digitalWrite(PIN_LED, LOW);
		LED_STATE = 0;
	}
}

